/*
  wxTriangulator
  Copyright 2008 Boleslaw Kulbabinski

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "globals.hpp"
#include "algorithms.hpp"

/*
  utilities
*/

// turn right = negative
inline long CrossProduct(long &x1, long &y1, long &x2, long &y2, long &x3, long &y3)
{
	return (x2 - x1) * (y3 - y2) - (x3 - x2) * (y2 - y1);
}

// cross product of vectors v1v2 and v2v3
long CrossProduct(Point v1, Point v2, Point v3)
{
	return CrossProduct(v1.x, v1.y, v2.x, v2.y, v3.x, v3.y);
}

// cross product of vectors v1v2 and v2v3
long CrossProduct(TriVertex *v1, TriVertex *v2, TriVertex *v3)
{
	return CrossProduct(v1->x, v1->y, v2->x, v2->y, v3->x, v3->y);
}

/*
  data structure for storing the polygon and its triangulation.
  every side is represented by a chain of edges, surrounding it in an anti-clockwise direction
  i.e. when we traverse through the consecutive edges, the side lies on our left.
*/

TriStructure::TriStructure(pair<VP_c_iter, VP_c_iter> init)
{
	Init(init);
}

TriStructure::~TriStructure()
{
	for(unsigned int i = 0; i < v_array.size(); i++)
		delete v_array[i];

	for(unsigned int i = 0; i < e_array.size(); i++)
		delete e_array[i];
}
/*
  creates the initial representation of the polygon, i.e. two doubly-linked lists of edges.
  exactly one chain of edges "surrounds" the "outer" infinite side of the polygon (real_side = false)
  It is important that the "link" pointer of each vertex is initially set to the edge belonging to that "unreal" side.
*/
void TriStructure::Init(pair<VP_c_iter, VP_c_iter> init)
{
	unsigned int i = 0;

	for(VP_c_iter it = init.first; it != init.second; it++, i++)
	{
		v_array.push_back(new TriVertex);
		v_array.back()->index = i;
		v_array.back()->x = it->x;
		v_array.back()->y = it->y;
	}

	// first cycle
	for(i = 0; i < v_array.size(); i++)
	{
		TriEdge *new_edge = new TriEdge;
		e_array.push_back(new_edge);

		new_edge->origin = v_array[i];
		if(i != 0)
		{
			new_edge->next = v_array[i-1]->link;
			new_edge->next->prev = new_edge;
		}

		v_array[i]->link = new_edge;
	}
	v_array.front()->link->next = v_array.back()->link;
	v_array.back()->link->prev = v_array.front()->link;

	// second cycle
	for(i = 0; i < v_array.size(); i++)
	{
		TriEdge *new_edge = new TriEdge;
		e_array.push_back(new_edge);

		new_edge->origin = v_array[i];
		new_edge->sibling = v_array[(i + 1) % v_array.size()]->link;
		new_edge->sibling->sibling = new_edge;

		if(i != 0)
		{
			new_edge->sibling->next->sibling->next = new_edge;
			new_edge->prev = new_edge->sibling->next->sibling;
		}
	}
	v_array.front()->link->sibling->next = v_array.front()->link->prev->sibling;
	v_array.front()->link->prev->sibling->prev = v_array.front()->link->sibling;

	// direction
	bool clockwise = test_clockwise_order(&v_array);

	for(vector<TriVertex*>::iterator it = v_array.begin(); it != v_array.end(); it++)
	{
		if(clockwise)
		{
			// vertices were given in clockwise order so "link" pointers should be changed into opposite
			(*it)->link = (*it)->link->sibling->next;
		}
		(*it)->link->real_side = false;
	}
}

void TriStructure::SortVertices(bool (*comparator)(TriVertex*, TriVertex*))
{
	sort(v_array.begin(), v_array.end(), comparator);
}

void TriStructure::AddEdge(TriVertex* v1, TriVertex *v2)
{
	// find correct place for the new vertex in neighbour lists
	while(!test_clockwise_order(v1, v1->link->sibling->origin, v2, v1->link->sibling->next->sibling->origin))
	{
		v1->link = v1->link->sibling->next;
	}

	while(!test_clockwise_order(v2, v2->link->sibling->origin, v1, v2->link->sibling->next->sibling->origin))
	{
		v2->link = v2->link->sibling->next;
	}

	TriEdge *new_edge_1 = new TriEdge;
	e_array.push_back(new_edge_1);
	TriEdge *new_edge_2 = new TriEdge;
	e_array.push_back(new_edge_2);

	// v1  -->  v2
	new_edge_1->real_side = true;
	new_edge_1->processed = false;
	new_edge_1->origin = v1;

	new_edge_1->prev = v1->link->sibling;
	new_edge_1->next = v2->link->sibling->next;

	new_edge_1->sibling = new_edge_2;

	// v2  -->  v1
	new_edge_2->real_side = true;
	new_edge_2->processed = false;
	new_edge_2->origin = v2;

	new_edge_2->prev = v2->link->sibling;
	new_edge_2->next = v1->link->sibling->next;

	new_edge_2->sibling = new_edge_1;

	// v1
	v1->link->sibling->next = new_edge_1;
	v1->link->prev->sibling->prev = new_edge_2;

	// v2
	v2->link->sibling->next = new_edge_2;
	v2->link->prev->sibling->prev = new_edge_1;
}

pair<VTV_c_iter, VTV_c_iter> TriStructure::GetVertices()
{
	return make_pair<VTV_c_iter, VTV_c_iter>(v_array.begin(), v_array.end());
}

/*
  returns true if vertices v1,v2,v3 lie in clockwise order around vertex "centre"
  actually tests whether v2 lies inside angle v1-centre-v3
*/
bool TriStructure::test_clockwise_order(TriVertex *centre, TriVertex *v1, TriVertex *v2, TriVertex *v3)
{
	if(CrossProduct(v1, centre, v3) >= 0)
	{
		// convex angle, test if v2 lies on the left of v1-centre and on the left of centre-v3
		return (CrossProduct(v1, centre, v2) >= 0 && CrossProduct(centre, v3, v2) >= 0); // XXX: >= ?
	}
	else
	{
		// reflex angle, test if v2 does NOT lie on the left of centre-v1 and on the left of v3-centre
		return !(CrossProduct(centre, v1, v2) >= 0 && CrossProduct(v3, centre, v2) >= 0); // XXX: >= ?
	}
}

/*
  return true if points are given in clockwise order
*/
bool TriStructure::test_clockwise_order(vector<TriVertex*> *points)
{
	unsigned int highest = 0;
	for(unsigned int i = 1; i < points->size(); i++)
	{
		if((*points)[i]->y > (*points)[highest]->y ||
		  ((*points)[i]->y == (*points)[highest]->y && (*points)[i]->x < (*points)[highest]->x))
		{
			highest = i;
		}
	}

	// if we turn right while passing the highest vertex, the order is clockwise
	if(CrossProduct((*points)[(highest + points->size() - 1) % points->size()],
	                (*points)[highest],
			(*points)[(highest + 1) % points->size()]) < 0)
	{
		return true;
	}
	else
		return false;
}

/*
  algorithm class for:
    1) dividing the polygon into y-monotone sub-polygons
    2) triangulating the y-monotone pieces
*/

/*
  comparator for storing the edges on multiset
  true if A < B
*/
bool SegmentCompare::operator()(TriEdge *A, TriEdge *B)
{
	Fraction a = x_coordinate(A, TriAlgorithm::sweepline_pos);
	Fraction b = x_coordinate(B, TriAlgorithm::sweepline_pos);
	if(a == b)
	{
		TriVertex *A_min = A->origin;
		TriVertex *A_max = A->sibling->origin;
		if(VertexCompare(A_min, A_max))
			swap(A_min, A_max);

		TriVertex *B_min = B->origin;
		TriVertex *B_max = B->sibling->origin;
		if(VertexCompare(B_min, B_max))
			swap(B_min, B_max);

		if((A_min == B_min && CrossProduct(A_max, A_min, B_max) > 0) ||
		   (A_max == B_max && CrossProduct(A_min, A_max, B_min) < 0))
		{
			return true;
		}
		else
			return false;
	}
	else
		return a < b;
}

Fraction SegmentCompare::x_coordinate(TriEdge *edge, long Y)
{
	Fraction X;

	long x1 = edge->origin->x;
	long y1 = edge->origin->y;
	long x2 = edge->sibling->origin->x;
	long y2 = edge->sibling->origin->y;

	if(y1 == y2)
	{
		X.numerator = min(x1,x2);
		X.denominator = 1;
	}
	else
	{
		X.numerator = (Y - y2) * (x1 - x2) + x2 * (y1 - y2);
		X.denominator = y1 - y2;
		if(X.denominator < 0)
		{
			X.denominator = -X.denominator;
			X.numerator = -X.numerator;
		}
	}

	return X;
}

/*
  comparator for sorting the vertices, true if A > B
*/
bool VertexCompare(TriVertex *A, TriVertex *B)
{
	if(A->y == B->y)
		return A->x < B->x;
	else
		return A->y > B->y;
}

/*
  TriAlgorithm - main algorithm class

  init_status legend:
    -1 = not inited
     0 = inited without errors
     1 = TriStructure init error
     2 = found two equal vertices
     3 = the polygon is self-intersecting
*/
long TriAlgorithm::sweepline_pos;

TriAlgorithm::TriAlgorithm()
{
	init_status = -1;
}

TriAlgorithm::TriAlgorithm(pair<VP_c_iter, VP_c_iter> init)
{
	init_status = -1;
	Init(init);
}

void TriAlgorithm::Init(pair<VP_c_iter, VP_c_iter> init)
{
	outcome = new vector<pair<int, int> >;

	try
	{
		polygon = new TriStructure(init);
	}
	catch(string error)
	{
		init_error = error;
		init_status = 1;
		polygon_size = 0;
		throw init_error;
	}

	polygon_size = polygon->GetVertices().second - polygon->GetVertices().first;

	polygon->SortVertices(VertexCompare);

	if(!initial_test())
	{
		throw init_error;
	}
	else if(!self_intersections_test())
	{
		init_status = 3;
		init_error = "The polygon is self-intersecting !";
		throw init_error;
	}
	else
	{
		init_status = 0;
	}
}

void TriAlgorithm::Run()
{
	if(init_status != 0)
	{
		throw init_error;
	}

	outcome->clear();
	divide_into_monotone();

	pair<VTV_c_iter, VTV_c_iter> vertexes = polygon->GetVertices();

	VTV_c_iter v_begin = vertexes.first;
	VTV_c_iter v_end = vertexes.second;

	for(VTV_c_iter iter = v_begin; iter != v_end; iter++)
	{
		TriEdge *first = (*iter)->link;
		do
		{
			if((*iter)->link->real_side && !(*iter)->link->processed)
			{
				triangulate_monotone((*iter)->link);
			}
			(*iter)->link = (*iter)->link->sibling->next;
		}
		while((*iter)->link != first);
	}

	for(unsigned int i = 0; i < waiting_edges.size(); i++)
	{
		add_edge(waiting_edges[i].first, waiting_edges[i].second);
	}

	if(outcome->size() != polygon_size - 3)
	{
		string error("Wrong number of diagonals !");
		throw error;
	}
}

vector<pair<int, int> >* TriAlgorithm::GetOutcome()
{
	return outcome;
}

bool TriAlgorithm::initial_test()
{
	VTV_c_iter p_begin = polygon->GetVertices().first;
	VTV_c_iter p_end = polygon->GetVertices().second;

	// step 1: check if some two vertices are equal
	VTV_c_iter p_prev = p_begin;
	for(VTV_c_iter it = p_prev + 1; it != p_end; it++, p_prev++)
	{
		if((*it)->x == (*p_prev)->x && (*it)->y == (*p_prev)->y)
		{
			init_status = 2;
			init_error = "Found two equal vertices !";
			return false;
		}
	}

	// step 2: search for 0 or 360 degree angles
	for(VTV_c_iter it = p_begin; it != p_end; it++)
	{
		TriVertex *act = (*it);
		TriVertex *prev = act->link->sibling->origin;
		TriVertex *next = act->link->prev->origin;

		if(CrossProduct(prev, act, next) == 0)
		{
			if(act->x < min(prev->x, next->x) || act->x > max(prev->x, next->x) ||
			   act->y < min(prev->y, next->y) || act->y > max(prev->y, next->y))
			{
				init_status = 3;
				init_error = "Two consecutive edges overlap !";
				return false;
			}
		}
	}

	return true;
}

bool TriAlgorithm::self_intersections_test()
{
	multiset<TriEdge*, SegmentCompare> sweepline;

	VTV_c_iter p_begin = polygon->GetVertices().first;
	VTV_c_iter p_end = polygon->GetVertices().second;

	for(VTV_c_iter it = p_begin; it != p_end; it++)
	{
		TriAlgorithm::sweepline_pos = (*it)->y;

		TriEdge *e1 = (*it)->link;
		TriEdge *e2 = (*it)->link->sibling->next;

		// deletion
		if(e1->on_sweep)
		{
			if(!erase_edge(e1, sweepline))
				return false;
		}
		else
			e1->processed = true;

		if(e2->on_sweep)
		{
			if(!erase_edge(e2, sweepline))
				return false;
		}
		else
			e2->processed = true;

		// insertion
		if(e1->processed && !insert_edge(e1, sweepline))
			return false;
		if(e2->processed && !insert_edge(e2, sweepline))
			return false;
	}

	return true;
}

/*
  true if no intersections detected
*/
bool TriAlgorithm::insert_edge(TriEdge *edge, multiset<TriEdge*, SegmentCompare>& sweepline)
{
	multiset<TriEdge*, SegmentCompare>::iterator it, it2;
	it = sweepline.insert(edge);

	edge->sweep_pointer = it;
	edge->on_sweep = true;
	edge->processed = false;
	edge->sibling->sweep_pointer = it;
	edge->sibling->on_sweep = true;

	if(it != sweepline.begin())
	{
		it2 = it;
		it2--;

		if(check_intersection(*it, *it2))
			return false;
	}

	it2 = it;
	it2++;

	if(it2 != sweepline.end())
	{
		if(check_intersection(*it, *it2))
			return false;
	}

	return true;
}
/*
  true if no intersections detected
*/
bool TriAlgorithm::erase_edge(TriEdge *edge, multiset<TriEdge*, SegmentCompare>& sweepline)
{
	multiset<TriEdge*, SegmentCompare>::iterator it, it2;
	it = edge->sweep_pointer;
	it2 = it;
	it2++;

	if(it != sweepline.begin() && it2 != sweepline.end())
	{
		it--;
		if(check_intersection(*it, *it2))
			return false;
	}

	sweepline.erase(edge->sweep_pointer);
	edge->on_sweep = false;
	edge->on_sweep = false;;

	return true;
}

/*
  true if given edges intersect
*/
bool TriAlgorithm::check_intersection(TriEdge *A, TriEdge *B)
{
	if(A->origin == B->origin || A->sibling->origin == B->origin ||
	   A->origin == B->sibling->origin || A->sibling->origin == B->sibling->origin)
		return false;

	long a1 = CrossProduct(A->origin, A->sibling->origin, B->origin);
	long a2 = CrossProduct(A->origin, A->sibling->origin, B->sibling->origin);

	if((a1 > 0 && a2 > 0) || (a1 < 0 && a2 < 0))
		return false;

	a1 = CrossProduct(B->origin, B->sibling->origin, A->origin);
	a2 = CrossProduct(B->origin, B->sibling->origin, A->sibling->origin);

	if((a1 > 0 && a2 > 0) || (a1 < 0 && a2 < 0))
		return false;

	return true;
}

void TriAlgorithm::mark_new_edge(TriVertex *v1, TriVertex *v2)
{
	waiting_edges.push_back(make_pair<TriVertex*,TriVertex*>(v1,v2));
}

void TriAlgorithm::add_edge(TriVertex *v1, TriVertex *v2)
{
	outcome->push_back(make_pair<int,int>(v1->index, v2->index));
	polygon->AddEdge(v1,v2);
}

/*
vertex_type: checks type of the given vertex:
  1. start vertex
  2. end vertex
  3. split vertex
  4. merge vertex
  5. ordinary vertex - polygon on the right
  6. ordinary vertex - polygon on the left
*/
int TriAlgorithm::vertex_type(TriVertex *vertex)
{
	TriVertex *prev = vertex->link->sibling->origin;
	TriVertex *next = vertex->link->prev->origin;

	if(!VertexCompare(vertex, prev))
	{
		// previous vertex lies higher
		if(!VertexCompare(vertex, next))
		{
			// next vertex lies higher
			if(CrossProduct(prev, vertex, next) > 0)
			{
				// we turn left
				return 2;
			}
			else
			{
				// we turn right
				return 4;
			}
		}
		else
		{
			// next vertex lies lower
			return 5;
		}
	}
	else
	{
		// previous vertex lies lower
		if(!VertexCompare(vertex, next))
		{
			// next vertex lies higher
			return 6;
		}
		else
		{
			// next vertex lies lower
			if(CrossProduct(prev, vertex, next) > 0)
			{
				// we turn left
				return 1;
			}
			else
			{
				// we turn right
				return 3;
			}
			return 5;
		}

	}
}

void TriAlgorithm::divide_into_monotone()
{
	multiset<TriEdge*, SegmentCompare> sweepline;

	pair<VTV_c_iter, VTV_c_iter> vertexes = polygon->GetVertices();
	VTV_c_iter v_begin = vertexes.first;
	VTV_c_iter v_end = vertexes.second;

	// create a fake edge for upper_bound
	TriEdge *temp_edge = new TriEdge;
	temp_edge->origin = new TriVertex;
	temp_edge->sibling = new TriEdge;
	temp_edge->sibling->origin = new TriVertex;

	// main loop
	for(VTV_c_iter iter = v_begin; iter != v_end; iter++)
	{
		// fake edge
		temp_edge->origin->x = (*iter)->x;
		temp_edge->sibling->origin->x = (*iter)->x;
		temp_edge->origin->y = (*iter)->y;
		temp_edge->sibling->origin->y = (*iter)->y;

		TriAlgorithm::sweepline_pos = (*iter)->y;

		(*iter)->type = vertex_type(*iter);
		TriEdge *e_prev = (*iter)->link->sibling;
		TriEdge *e_next = (*iter)->link->sibling->next;
		switch((*iter)->type)
		{
			case 1: // start vertex
			{
				e_next->helper = (*iter);
				e_next->sweep_pointer = sweepline.insert(e_next);
				break;
			}
			case 2: // end vertex
			{
				if(e_prev->helper->type == 4)
				{
					add_edge(*iter, e_prev->helper);
				}
				sweepline.erase(e_prev->sweep_pointer);
				break;
			}
			case 3: // split vertex
			{
				multiset<TriEdge*, SegmentCompare>::iterator neighbour = sweepline.upper_bound(temp_edge);
				neighbour--;

				add_edge(*iter, (*neighbour)->helper);
				(*neighbour)->helper = *iter;

				e_next->helper = (*iter);
				e_next->sweep_pointer = sweepline.insert(e_next);
				break;
			}
			case 4: // merge vertex
			{
				if(e_prev->helper->type == 4)
				{
					add_edge(*iter, e_prev->helper);
				}
				sweepline.erase(e_prev->sweep_pointer);

				multiset<TriEdge*, SegmentCompare>::iterator neighbour = sweepline.upper_bound(temp_edge);
				neighbour--;

				if((*neighbour)->helper->type == 4)
				{
					add_edge(*iter, (*neighbour)->helper);
				}

				(*neighbour)->helper = *iter;
				break;
			}
			case 5: // ordinary vertex, polygon on its right
			{
				if(e_prev->helper->type == 4)
				{
					add_edge(*iter, e_prev->helper);
				}
				sweepline.erase(e_prev->sweep_pointer);

				e_next->helper = *iter;
				e_next->sweep_pointer = sweepline.insert(e_next);
				break;
			}
			case 6: // ordinary vertex, polygon on its left
			{
				multiset<TriEdge*, SegmentCompare>::iterator neighbour = sweepline.upper_bound(temp_edge);
				neighbour--;

				if((*neighbour)->helper->type == 4)
				{
					add_edge(*iter, (*neighbour)->helper);
				}

				(*neighbour)->helper = *iter;
				break;
			}
		}
	}

	// delete temp_edge
	delete temp_edge->sibling->origin;
	delete temp_edge->sibling;
	delete temp_edge->origin;
	delete temp_edge;
}

/*
  Traverse through the boundary of a y-monotone polygon, starting from the given edge,
  add vertices to vertex_list and set their types, which will help in identifying if
  two vertices are on the same side. Then run the greedy triangulation algorithm
*/
void TriAlgorithm::triangulate_monotone(TriEdge* boundary)
{
	vector<TriVertex*> vertex_list;

	for(; !boundary->processed; boundary = boundary->next)
	{
		boundary->processed = true;
		vertex_list.push_back(boundary->origin);
		if(VertexCompare(boundary->origin, boundary->sibling->origin))
		{
			boundary->origin->type = 5;
		}
		else
		{
			boundary->origin->type = 6;
		}
	}

	sort(vertex_list.begin(), vertex_list.end(), VertexCompare);
	vertex_list.back()->type = 2;

	// actual algorithm
	stack<TriVertex*> v_stack;
	v_stack.push(vertex_list[0]);
	v_stack.push(vertex_list[1]);

	for(unsigned int i = 2; i < vertex_list.size(); i++)
	{
		if(vertex_list[i]->type == 2 || vertex_list[i]->type == v_stack.top()->type)
		{
			// same side (the bottom-most vertex belongs to both sides)
			TriVertex *last_pop = v_stack.top();
			v_stack.pop();

			while(!v_stack.empty())
			{
				if(v_stack.size() == 1 && vertex_list[i]->type == 2)
					break;

				long direction = CrossProduct(vertex_list[i], v_stack.top(), last_pop);
				if((last_pop->type == 5 && direction >= 0) || (last_pop->type == 6 && direction <= 0)) // XXX: >= ?
				{
					mark_new_edge(vertex_list[i], v_stack.top());
					last_pop = v_stack.top();
					v_stack.pop();
				}
				else
					break;
			}

			v_stack.push(last_pop);
			v_stack.push(vertex_list[i]);
		}
		else
		{
			// opposite side
			while(v_stack.size() != 1)
			{
				mark_new_edge(vertex_list[i], v_stack.top());
				v_stack.pop();
			}

			v_stack.pop();
			v_stack.push(vertex_list[i-1]);
			v_stack.push(vertex_list[i]);
		}
	}
}

